//#define ASTARDEBUG

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using Pathfinding;
using Pathfinding.Voxels;

namespace Pathfinding.Voxels {
	public partial class Voxelize {
		
		public void BuildContours (float maxError, int maxEdgeLength, VoxelContourSet cset, int buildFlags) {
			
			AstarProfiler.StartProfile ("Build Contours");
			
			AstarProfiler.StartProfile ("- Init");
			int w = voxelArea.width;
			int d = voxelArea.depth;
			
			int wd = w*d;
			
			//cset.bounds = voxelArea.bounds;
			
			int maxContours = Mathf.Max (8/*Max Regions*/,8);
			
			
			//cset.conts = new VoxelContour[maxContours];
			List<VoxelContour> contours = new List<VoxelContour>(maxContours);
			
			AstarProfiler.EndProfile ("- Init");
			AstarProfiler.StartProfile ("- Mark Boundaries");
			
			//cset.nconts = 0;
			
			//NOTE: This array may contain any data, but since we explicitly set all data in it before we use it, it's OK.
			ushort[] flags = voxelArea.tmpUShortArr;
			if (flags.Length < voxelArea.compactSpanCount) {
				flags = voxelArea.tmpUShortArr = new ushort[voxelArea.compactSpanCount];
			}
			
			// Mark boundaries. (@?)
			for (int z=0;z < wd;z += voxelArea.width) {
				for (int x=0;x < voxelArea.width;x++) {
					
					CompactVoxelCell c = voxelArea.compactCells[x+z];
					
					for (int i= (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) {
						
						ushort res = 0;
						CompactVoxelSpan s = voxelArea.compactSpans[i];
						
						if (s.reg == 0 || (s.reg & BorderReg) == BorderReg) {
							flags[i] = 0;
							continue;
						}
						
						for (int dir=0;dir < 4; dir++) {
							int r = 0;
							
							if (s.GetConnection (dir) != NotConnected) {
								int nx = x + voxelArea.DirectionX[dir];
								int nz = z + voxelArea.DirectionZ[dir];
								
								int ni = (int)voxelArea.compactCells[nx+nz].index + s.GetConnection (dir);
								r = voxelArea.compactSpans[ni].reg;
								
								
							}
							
							//@TODO - Why isn't this inside the previous IF
							if (r == s.reg) {
								res |= (ushort)(1 << dir);
								
							}
						}
						
						//Inverse, mark non connected edges.
						flags[i] = (ushort)(res ^ 0xf);
							
					}
					
				}
			}
			
			AstarProfiler.EndProfile ("- Mark Boundaries");
			
			AstarProfiler.StartProfile ("- Simplify Contours");
			List<int> verts = Pathfinding.Util.ListPool<int>.Claim(256);//new List<int> (256);
			List<int> simplified = Pathfinding.Util.ListPool<int>.Claim(64);//new List<int> (64);
			
			for (int z=0;z < wd;z += voxelArea.width) {
				for (int x=0;x < voxelArea.width;x++) {
					
					CompactVoxelCell c = voxelArea.compactCells[x+z];
					
					for (int i= (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) {
						
						//CompactVoxelSpan s = voxelArea.compactSpans[i];
						
						if (flags[i] == 0 || flags[i] == 0xf)
						{
							flags[i] = 0;
							continue;
						}
						
						int reg = voxelArea.compactSpans[i].reg;
						
						if (reg == 0 || (reg & BorderReg) == BorderReg) {
							continue;
						}
						
						int area = voxelArea.areaTypes[i];
						
						verts.Clear ();
						simplified.Clear ();
						
						WalkContour(x, z, i, flags, verts);
						
						SimplifyContour(verts, simplified, maxError, maxEdgeLength, buildFlags);
						RemoveDegenerateSegments (simplified);
						
						VoxelContour contour = new VoxelContour ();
						contour.verts = ClaimIntArr(simplified.Count,false);//simplified.ToArray ();
						for (int j=0;j<simplified.Count;j++) contour.verts[j] = simplified[j];
#if ASTAR_RECAST_INCLUDE_RAW_VERTEX_CONTOUR
						//Not used at the moment, just debug stuff
						contour.rverts = ClaimIntArr(verts.Count);
						for (int j=0;j<verts.Count;j++) contour.rverts[j] = verts[j];
#endif
						contour.nverts = simplified.Count/4;
						contour.reg = reg;
						contour.area = area;
						
						contours.Add (contour);
						
						#if ASTARDEBUG
						for (int q=0, j = (simplified.Count/4)-1;q<(simplified.Count/4);j=q, q++) {
				
							int i4 = q*4;
							int j4 = j*4;
							
							Vector3 p1 = Vector3.Scale (
							new Vector3 (
								simplified[i4+0],
								simplified[i4+1],
								(simplified[i4+2]/(float)voxelArea.width)
							),
							cellScale)
							+voxelOffset;
							
							Vector3 p2 = Vector3.Scale (
							new Vector3 (
								simplified[j4+0],
								simplified[j4+1],
								(simplified[j4+2]/(float)voxelArea.width)
							)
							,cellScale)
							+voxelOffset;
							
							
							if (CalcAreaOfPolygon2D(contour.verts, contour.nverts) > 0) {
								Debug.DrawLine (p1,p2,Mathfx.IntToColor (reg,0.5F));
							} else {
								Debug.DrawLine (p1,p2,Color.red);
								
							}
							
						}
						#endif
					}
				}
			}
			
			Pathfinding.Util.ListPool<int>.Release(verts);
			Pathfinding.Util.ListPool<int>.Release(simplified);
			
			AstarProfiler.EndProfile ("- Simplify Contours");
			
			AstarProfiler.StartProfile ("- Fix Contours");
			
			// Check and merge droppings.
			// Sometimes the previous algorithms can fail and create several contours
			// per area. This pass will try to merge the holes into the main region.
			for (int i = 0; i < contours.Count; i++)
			{
				VoxelContour cont = contours[i];
				// Check if the contour is would backwards.
				if (CalcAreaOfPolygon2D(cont.verts, cont.nverts) < 0)
				{
					// Find another contour which has the same region ID.
					int mergeIdx = -1;
					for (int j = 0; j < contours.Count; j++)
					{
						if (i == j) continue;
						if (contours[j].nverts > 0 && contours[j].reg == cont.reg)
						{
							// Make sure the polygon is correctly oriented.
							if (CalcAreaOfPolygon2D(contours[j].verts, contours[j].nverts) > 0)
							{
								mergeIdx = j;
								break;
							}
						}
					}
					if (mergeIdx == -1)
					{
						Debug.LogError ("rcBuildContours: Could not find merge target for bad contour "+i+".");
					}
					else
					{
						// Debugging
						//Debug.LogWarning ("Fixing contour");

						VoxelContour mcont = contours[mergeIdx];
						// Merge by closest points.
						int ia = 0, ib = 0;
						GetClosestIndices(mcont.verts, mcont.nverts, cont.verts, cont.nverts, ref ia, ref ib);
						
						if (ia == -1 || ib == -1)
						{
							Debug.LogWarning ("rcBuildContours: Failed to find merge points for "+i+" and "+mergeIdx+".");
							continue;
						}
						
#if ASTARDEBUG
						int p4 = ia*4;
						int p42 = ib*4;
							
						Vector3 p12 = Vector3.Scale (
							new Vector3 (
								mcont.verts[p4+0],
								mcont.verts[p4+1],
								(mcont.verts[p4+2]/(float)voxelArea.width)
							),
							cellScale)
							+voxelOffset;
							
							Vector3 p22 = Vector3.Scale (
							new Vector3 (
								cont.verts[p42+0],
								cont.verts[p42+1],
								(cont.verts[p42+2]/(float)voxelArea.width)
							)
							,cellScale)
							+voxelOffset;
						
						Debug.DrawLine (p12,p22,Color.green);
#endif
						
						if (!MergeContours(ref mcont, ref cont, ia, ib))
						{
							Debug.LogWarning ("rcBuildContours: Failed to merge contours "+i+" and "+mergeIdx+".");
							continue;
						}
						
						contours[mergeIdx] = mcont;
						contours[i] = cont;
						
						#if ASTARDEBUG
						Debug.Log (mcont.nverts);
						
						for (int q=0, j = (mcont.nverts)-1;q<(mcont.nverts);j=q, q++) {
							int i4 = q*4;
							int j4 = j*4;
							
							Vector3 p1 = Vector3.Scale (
							new Vector3 (
								mcont.verts[i4+0],
								mcont.verts[i4+1],
								(mcont.verts[i4+2]/(float)voxelArea.width)
							),
							cellScale)
							+voxelOffset;
							
							Vector3 p2 = Vector3.Scale (
							new Vector3 (
								mcont.verts[j4+0],
								mcont.verts[j4+1],
								(mcont.verts[j4+2]/(float)voxelArea.width)
							)
							,cellScale)
							+voxelOffset;
							
							Debug.DrawLine (p1,p2,Color.red);
						//}
						}
						#endif
					}
				}
			}
		
			cset.conts = contours;
			
			AstarProfiler.EndProfile ("- Fix Contours");
			
			AstarProfiler.EndProfile ("Build Contours");
		}
		
		void GetClosestIndices(int[] vertsa, int nvertsa,
								  int[] vertsb, int nvertsb,
								  ref int ia, ref int ib) {
								  	
			int closestDist = 0xfffffff;
			ia = -1;
			ib = -1;
			for (int i = 0; i < nvertsa; i++)
			{
				//in is a keyword in C#, so I can't use that as a variable name
				int in2 = (i+1) % nvertsa;
				int ip = (i+nvertsa-1) % nvertsa;
				int va = i*4;
				int van = in2*4;
				int vap = ip*4;
				
				for (int j = 0; j < nvertsb; ++j)
				{
					int vb = j*4;
					// vb must be "infront" of va.
					if (Ileft(vap,va,vb,vertsa,vertsa,vertsb) && Ileft(va,van,vb,vertsa,vertsa,vertsb))
					{
						int dx = vertsb[vb+0] - vertsa[va+0];
						int dz = (vertsb[vb+2]/voxelArea.width) - (vertsa[va+2]/voxelArea.width);
						int d = dx*dx + dz*dz;
						if (d < closestDist)
						{
							ia = i;
							ib = j;
							closestDist = d;
						}
					}
				}
			}
		}
		
		static List<int[]> intArrCache = new List<int[]>();
		static readonly int[] emptyArr = new int[0];
		
		static void ReleaseIntArr (int[] arr) {
			if (arr != null) intArrCache.Add (arr);
		}
		
		static int[] ClaimIntArr (int minCapacity, bool zero) {
			for (int i=0;i<intArrCache.Count;i++) {
				if (intArrCache[i].Length >= minCapacity) {
					int[] arr = intArrCache[i];
					intArrCache.RemoveAt (i);
					if (zero) Pathfinding.Util.Memory.MemSet<int>(arr,0,sizeof(int));
					return arr;
				}
			}
			return new int[minCapacity];
		}
		
		/** Releases contents of a contour set to caches */
		static void ReleaseContours (VoxelContourSet cset) {
			for (int i=0;i<cset.conts.Count;i++) {
				VoxelContour cont = cset.conts[i];
				ReleaseIntArr(cont.verts);
				ReleaseIntArr(cont.rverts);
			}
			cset.conts = null;
		}
		
		public static bool MergeContours(ref VoxelContour ca, ref VoxelContour cb, int ia, int ib) {
			
			int maxVerts = ca.nverts + cb.nverts + 2;
			
			int[] verts = ClaimIntArr(maxVerts*4,false);//new int[maxVerts*4];
			
			//if (!verts)
			//	return false;
		
			int nv = 0;
		
			// Copy contour A.
			for (int i = 0; i <= ca.nverts; i++)
			{
				int dst = nv*4;
				int src = ((ia+i) % ca.nverts)*4;
				verts[dst+0] = ca.verts[src+0];
				verts[dst+1] = ca.verts[src+1];
				verts[dst+2] = ca.verts[src+2];
				verts[dst+3] = ca.verts[src+3];
				nv++;
			}
		
			// Copy contour B
			for (int i = 0; i <= cb.nverts; i++)
			{
				int dst = nv*4;
				int src = ((ib+i) % cb.nverts)*4;
				verts[dst+0] = cb.verts[src+0];
				verts[dst+1] = cb.verts[src+1];
				verts[dst+2] = cb.verts[src+2];
				verts[dst+3] = cb.verts[src+3];
				nv++;
			}
			
			//rcFree(ca.verts);
			//rcFree(cb.verts);
			ReleaseIntArr(ca.verts);
			ReleaseIntArr(cb.verts);
			
			ca.verts = verts;
			ca.nverts = nv;
			
			cb.verts = emptyArr;
			cb.nverts = 0;
			
			return true;
		}
		
		public void SimplifyContour(List<int> verts, List<int> simplified, float maxError, int maxEdgeLenght, int buildFlags) {
			
			// Add initial points.
			bool hasConnections = false;
			for (int i = 0; i < verts.Count; i += 4) {
				
				if ((verts[i+3] & ContourRegMask) != 0) {
					
					hasConnections = true;
					break;
				}
			}
			
			if (hasConnections) {
				
				// The contour has some portals to other regions.
				// Add a new point to every location where the region changes.
				for (int i = 0, ni = verts.Count/4; i < ni; i++) {
					
					int ii = (i+1) % ni;
					bool differentRegs = (verts[i*4+3] & ContourRegMask) != (verts[ii*4+3] & ContourRegMask);
					bool areaBorders = (verts[i*4+3] & RC_AREA_BORDER) != (verts[ii*4+3] & RC_AREA_BORDER);
					
					if (differentRegs || areaBorders)
					{
						simplified.Add(verts[i*4+0]);
						simplified.Add(verts[i*4+1]);
						simplified.Add(verts[i*4+2]);
						simplified.Add(i);
					}
				}
			}
			
			
			if (simplified.Count == 0) {
				
				// If there is no connections at all,
				// create some initial points for the simplification process. 
				// Find lower-left and upper-right vertices of the contour.
				int llx = verts[0];
				int lly = verts[1];
				int llz = verts[2];
				int lli = 0;
				int urx = verts[0];
				int ury = verts[1];
				int urz = verts[2];
				int uri = 0;
				
				for (int i = 0; i < verts.Count; i += 4)
				{
					int x = verts[i+0];
					int y = verts[i+1];
					int z = verts[i+2];
					if (x < llx || (x == llx && z < llz))
					{
						llx = x;
						lly = y;
						llz = z;
						lli = i/4;
					}
					if (x > urx || (x == urx && z > urz))
					{
						urx = x;
						ury = y;
						urz = z;
						uri = i/4;
					}
				}
				
				simplified.Add(llx);
				simplified.Add(lly);
				simplified.Add(llz);
				simplified.Add(lli);
				
				simplified.Add(urx);
				simplified.Add(ury);
				simplified.Add(urz);
				simplified.Add(uri);
			}
			
			// Add points until all raw points are within
			// error tolerance to the simplified shape.
			int pn = verts.Count/4;
			
			//Use the max squared error instead
			maxError *= maxError;
			
			for (int i = 0; i < simplified.Count/4; )
			{
				
				int ii = (i+1) % (simplified.Count/4);
				
				int ax = simplified[i*4+0];
				int az = simplified[i*4+2];
				int ai = simplified[i*4+3];
				
				int bx = simplified[ii*4+0];
				int bz = simplified[ii*4+2];
				int bi = simplified[ii*4+3];
				
				// Find maximum deviation from the segment.
				float maxd = 0;
				int maxi = -1;
				int ci, cinc, endi;
				
				// Traverse the segment in lexilogical order so that the
				// max deviation is calculated similarly when traversing
				// opposite segments.
				if (bx > ax || (bx == ax && bz > az))
				{
					cinc = 1;
					ci = (ai+cinc) % pn;
					endi = bi;
				}
				else
				{
					cinc = pn-1;
					ci = (bi+cinc) % pn;
					endi = ai;
				}
				
				// Tessellate only outer edges or edges between areas.
				if ((verts[ci*4+3] & ContourRegMask) == 0 ||
					(verts[ci*4+3] & RC_AREA_BORDER) == RC_AREA_BORDER) {
						
					while (ci != endi)
					{
						float d2 = AstarMath.DistancePointSegment (verts[ci*4+0], verts[ci*4+2]/voxelArea.width, ax, az/voxelArea.width, bx, bz/voxelArea.width);
						//float d2 = Mathfx.DistancePointSegment2 (verts[ci*4+0], verts[ci*4+2]/voxelArea.width, ax, az/voxelArea.width, bx, bz/voxelArea.width);
						
						//if (Mathf.Abs (d2-d3) > 0.5F) {
						//	Debug.Log (d2+" "+d3);
						//}
						
						if (d2 > maxd)
						{
							maxd = d2;
							maxi = ci;
						}
						ci = (ci+cinc) % pn;
					}
				}
				
				// If the max deviation is larger than accepted error,
				// add new point, else continue to next segment.
				if (maxi != -1 && maxd > maxError)
				{
					// Add space for the new point.
					//simplified.resize(simplified.size()+4);
					simplified.Add(0);
					simplified.Add(0);
					simplified.Add(0);
					simplified.Add(0);
					
					int n = simplified.Count/4;
					
					for (int j = n-1; j > i; --j)
					{
						simplified[j*4+0] = simplified[(j-1)*4+0];
						simplified[j*4+1] = simplified[(j-1)*4+1];
						simplified[j*4+2] = simplified[(j-1)*4+2];
						simplified[j*4+3] = simplified[(j-1)*4+3];
					}
					// Add the point.
					simplified[(i+1)*4+0] = verts[maxi*4+0];
					simplified[(i+1)*4+1] = verts[maxi*4+1];
					simplified[(i+1)*4+2] = verts[maxi*4+2];
					simplified[(i+1)*4+3] = maxi;
				}
				else
				{
					i++;
				}
			}
			
			
			
			//Split too long edges
			
			float maxEdgeLen = maxEdgeLength / cellSize;
			
			if (maxEdgeLen > 0 && (buildFlags & (RC_CONTOUR_TESS_WALL_EDGES|RC_CONTOUR_TESS_AREA_EDGES)) != 0) {
				
				for (int i = 0; i < simplified.Count/4; ) {
					
					if (simplified.Count/4 > 200) {
						break;
					}
					
					int ii = (i+1) % (simplified.Count/4);
					
					int ax = simplified[i*4+0];
					int az = simplified[i*4+2];
					int ai = simplified[i*4+3];
					
					int bx = simplified[ii*4+0];
					int bz = simplified[ii*4+2];
					int bi = simplified[ii*4+3];
					
					// Find maximum deviation from the segment.
					int maxi = -1;
					int ci = (ai+1) % pn;
	
					// Tessellate only outer edges or edges between areas.
					bool tess = false;
					
					// Wall edges.
					if ((buildFlags & RC_CONTOUR_TESS_WALL_EDGES) == 1 && (verts[ci*4+3] & ContourRegMask) == 0)
						tess = true;
					// Edges between areas.
					if ((buildFlags & RC_CONTOUR_TESS_AREA_EDGES) == 1 && (verts[ci*4+3] & RC_AREA_BORDER) == 1)
						tess = true;
					
					if (tess) {
						
						int dx = bx - ax;
						int dz = (bz/voxelArea.width) - (az/voxelArea.width);
						if (dx*dx + dz*dz > maxEdgeLen*maxEdgeLen) {
							
							// Round based on the segments in lexilogical order so that the
							// max tesselation is consistent regardles in which direction
							// segments are traversed.
							if (bx > ax || (bx == ax && bz > az))
							{
								int n = bi < ai ? (bi+pn - ai) : (bi - ai);
								maxi = (ai + n/2) % pn;
							}
							else
							{
								int n = bi < ai ? (bi+pn - ai) : (bi - ai);
								maxi = (ai + (n+1)/2) % pn;
							}
						}
						
						
					}
					
					// If the max deviation is larger than accepted error,
					// add new point, else continue to next segment.
					if (maxi != -1)
					{
						// Add space for the new point.
						//simplified.resize(simplified.size()+4);
						simplified.AddRange (new int[4]);
						
						int n = simplified.Count/4;
						for (int j = n-1; j > i; --j)
						{
							simplified[j*4+0] = simplified[(j-1)*4+0];
							simplified[j*4+1] = simplified[(j-1)*4+1];
							simplified[j*4+2] = simplified[(j-1)*4+2];
							simplified[j*4+3] = simplified[(j-1)*4+3];
						}
						// Add the point.
						simplified[(i+1)*4+0] = verts[maxi*4+0];
						simplified[(i+1)*4+1] = verts[maxi*4+1];
						simplified[(i+1)*4+2] = verts[maxi*4+2];
						simplified[(i+1)*4+3] = maxi;
					}
					else
					{
						++i;
					}
				}
			}
			
			for (int i = 0; i < simplified.Count/4; i++)
			{
				// The edge vertex flag is take from the current raw point,
				// and the neighbour region is take from the next raw point.
				int ai = (simplified[i*4+3]+1) % pn;
				int bi = simplified[i*4+3];
				simplified[i*4+3] = (verts[ai*4+3] & ContourRegMask) | (verts[bi*4+3] & RC_BORDER_VERTEX);
			}
		}
		
		public void WalkContour (int x, int z, int i, ushort[] flags, List<int> verts) {
			
			// Choose the first non-connected edge
			int dir = 0;
			
			while ((flags[i] & (ushort)(1 << dir)) == 0) {
				dir++;
			}
			
			int startDir = dir;
			int startI = i;
			
			int area = voxelArea.areaTypes[i];
			
			int iter = 0;
			
			#if ASTARDEBUG
			Vector3 previousPos;
			Vector3 currentPos;
			
			previousPos = ConvertPos (
				x,
				0,
				z
			);
			
			Vector3 previousPos2 = ConvertPos (
				x,
				0,
				z
			);
			#endif
			
			while (iter++ < 40000) {
				
				//Are we facing a region edge
				if ((flags[i] & (ushort)(1 << dir)) != 0) {
					
					#if ASTARDEBUG
					Vector3 pos = ConvertPos (x,0,z)+new Vector3 ((voxelArea.DirectionX[dir] != 0) ? Mathf.Sign(voxelArea.DirectionX[dir]) : 0,0,(voxelArea.DirectionZ[dir]) != 0 ? Mathf.Sign(voxelArea.DirectionZ[dir]) : 0)*0.6F;
					int dir2 = (dir+1) & 0x3;
					//pos += new Vector3 ((voxelArea.DirectionX[dir2] != 0) ? Mathf.Sign(voxelArea.DirectionX[dir2]) : 0,0,(voxelArea.DirectionZ[dir2]) != 0 ? Mathf.Sign(voxelArea.DirectionZ[dir2]) : 0)*1.2F;
					
					//Debug.DrawLine (ConvertPos (x,0,z),pos,Color.cyan);
					// Debug.DrawLine (previousPos2,pos,Color.blue);
					previousPos2 = pos;
					#endif
					//Choose the edge corner
					
					bool isBorderVertex = false;
					bool isAreaBorder = false;
					
					int px = x;
					int py = GetCornerHeight (x,z,i,dir,ref isBorderVertex);
					int pz = z;
					
					switch(dir)
					{
						case 0: pz += voxelArea.width;; break;
						case 1: px++; pz += voxelArea.width; break;
						case 2: px++; break;
					}
					
					/*case 1: px++; break;
						case 2: px++; pz += voxelArea.width; break;
						case 3: pz += voxelArea.width; break;
					*/
					
					int r = 0;
					CompactVoxelSpan s = voxelArea.compactSpans[i];
					
					if (s.GetConnection (dir) != NotConnected)
					{
						int nx = x + voxelArea.DirectionX[dir];
						int nz = z + voxelArea.DirectionZ[dir];
						int ni = (int)voxelArea.compactCells[nx+nz].index + s.GetConnection (dir);
						r = (int)voxelArea.compactSpans[ni].reg;
						
						if (area != voxelArea.areaTypes[ni]) {
							isAreaBorder = true;
						}
					}
					
					if (isBorderVertex) {
						r |= RC_BORDER_VERTEX;
					}
					if (isAreaBorder) {
						r |= RC_AREA_BORDER;
					}
					
					verts.Add(px);
					verts.Add(py);
					verts.Add(pz);
					verts.Add(r);
					
					//Debug.DrawRay (previousPos,new Vector3 ((dir == 1 || dir == 2) ? 1 : 0, 0, (dir == 0 || dir == 1) ? 1 : 0),Color.cyan);
					
					flags[i] = (ushort)(flags[i] & ~(1 << dir)); // Remove visited edges
					
					dir = (dir+1) & 0x3;  // Rotate CW
					
				} else {
					int ni = -1;
					int nx = x + voxelArea.DirectionX[dir];
					int nz = z + voxelArea.DirectionZ[dir];
					
					CompactVoxelSpan s = voxelArea.compactSpans[i];
					
					if (s.GetConnection (dir) != NotConnected)
					{
						CompactVoxelCell nc = voxelArea.compactCells[nx+nz];
						ni = (int)nc.index + s.GetConnection(dir);
					}
					
					if (ni == -1)
					{
						Debug.LogWarning ("Degenerate triangles might have been generated.\n" +
							"Usually this is not a problem, but if you have a static level, try to modify the graph settings slightly to avoid this edge case.");
						return;
					}
					x = nx;
					z = nz;
					i = ni;
					
					dir = (dir+3) & 0x3;	// Rotate CCW
					
					#if ASTARDEBUG
					currentPos = ConvertPos (
						x,
						0,
						z
					);
					
					Debug.DrawLine (previousPos+Vector3.up*0,currentPos,Color.blue);
					previousPos = currentPos;
					#endif
				}
				
				if (startI == i && startDir == dir)
				{
					break;
				}
			}
			
			#if ASTARDEBUG
			Color col = new Color (Random.value,Random.value,Random.value);
			
			Vector3 offset = new Vector3 (0,Random.value*10,0);
			for (int q=0, j = (verts.Count/4)-1;q<(verts.Count/4);j=q, q++) {
				
				int i4 = q*4;
				int j4 = j*4;
				
				Vector3 p1 = ConvertPosWithoutOffset (
					verts[i4+0],
					verts[i4+1],
					verts[i4+2]
				);
				
				Vector3 p2 = ConvertPosWithoutOffset (
					verts[j4+0],
					verts[j4+1],
					verts[j4+2]
				);
				
				Debug.DrawLine (p1,p2,col);
				
			}
			#endif
			
		}
		
		public int GetCornerHeight (int x, int z, int i, int dir, ref bool isBorderVertex) {
			
			CompactVoxelSpan s = voxelArea.compactSpans[i];
			
			int ch = (int)s.y;
			
			//dir + clockwise direction
			int dirp = (dir+1) & 0x3;
			//int dirp = (dir+3) & 0x3;
			
			uint[] regs = new uint[4];
			
			regs[0] = (uint)voxelArea.compactSpans[i].reg | ((uint)voxelArea.areaTypes[i] << 16);
			
			if (s.GetConnection (dir) != NotConnected) {
				int nx = x + voxelArea.DirectionX[dir];
				int nz = z + voxelArea.DirectionZ[dir];
				int ni = (int)voxelArea.compactCells[nx+nz].index + s.GetConnection(dir);
				
				CompactVoxelSpan ns = voxelArea.compactSpans[ni];
				
				ch = AstarMath.Max (ch,(int)ns.y);
				regs[1] = (uint)ns.reg | ((uint)voxelArea.areaTypes[ni] << 16);
				
				if (ns.GetConnection (dirp) != NotConnected) {
					int nx2 = nx + voxelArea.DirectionX[dirp];
					int nz2 = nz + voxelArea.DirectionZ[dirp];
					int ni2 = (int)voxelArea.compactCells[nx2+nz2].index + ns.GetConnection(dirp);
					
					CompactVoxelSpan ns2 = voxelArea.compactSpans[ni2];
					
					ch = AstarMath.Max (ch,(int)ns2.y);
					regs[2] = (uint)ns2.reg | ((uint)voxelArea.areaTypes[ni2] << 16);
				}
			}
			
			if (s.GetConnection (dirp) != NotConnected) {
				int nx = x + voxelArea.DirectionX[dirp];
				int nz = z + voxelArea.DirectionZ[dirp];
				int ni = (int)voxelArea.compactCells[nx+nz].index + s.GetConnection(dirp);
				
				CompactVoxelSpan ns = voxelArea.compactSpans[ni];
				
				ch = AstarMath.Max (ch,(int)ns.y);
				regs[3] = (uint)ns.reg | ((uint)voxelArea.areaTypes[ni] << 16);
				
				if (ns.GetConnection (dir) != NotConnected) {
					int nx2 = nx + voxelArea.DirectionX[dir];
					int nz2 = nz + voxelArea.DirectionZ[dir];
					int ni2 = (int)voxelArea.compactCells[nx2+nz2].index + ns.GetConnection(dir);
					
					CompactVoxelSpan ns2 = voxelArea.compactSpans[ni2];
					
					ch = AstarMath.Max (ch,(int)ns2.y);
					regs[2] = (uint)ns2.reg | ((uint)voxelArea.areaTypes[ni2] << 16);
				}
			}
			
			// Check if the vertex is special edge vertex, these vertices will be removed later.
			for (int j = 0; j < 4; ++j) {
				int a = j;
				int b = (j+1) & 0x3;
				int c = (j+2) & 0x3;
				int d = (j+3) & 0x3;
				
				// The vertex is a border vertex there are two same exterior cells in a row,
				// followed by two interior cells and none of the regions are out of bounds.
				bool twoSameExts = (regs[a] & regs[b] & BorderReg) != 0 && regs[a] == regs[b];
				bool twoInts = ((regs[c] | regs[d]) & BorderReg) == 0;
				bool intsSameArea = (regs[c]>>16) == (regs[d]>>16);
				bool noZeros = regs[a] != 0 && regs[b] != 0 && regs[c] != 0 && regs[d] != 0;
				if (twoSameExts && twoInts && intsSameArea && noZeros)
				{
					isBorderVertex = true;
					break;
				}
			}
			
			return ch;
		}
		
		public void RemoveDegenerateSegments(List<int> simplified) {
			
			// Remove adjacent vertices which are equal on xz-plane,
			// or else the triangulator will get confused
			for (int i = 0; i < simplified.Count/4; i++) {
				
				int ni = i+1;
				if (ni >= (simplified.Count/4))
					ni = 0;
					
				if (simplified[i*4+0] == simplified[ni*4+0] &&
					simplified[i*4+2] == simplified[ni*4+2])
				{
					// Degenerate segment, remove.
					simplified.RemoveRange (i,4);
					
					/*for (int j = i; j < simplified.Count/4-1; ++j)
					{
						simplified[j*4+0] = simplified[(j+1)*4+0];
						simplified[j*4+1] = simplified[(j+1)*4+1];
						simplified[j*4+2] = simplified[(j+1)*4+2];
						simplified[j*4+3] = simplified[(j+1)*4+3];
					}
					simplified.resize(simplified.Count-4);*/
				}
			}
		}
		
		public int CalcAreaOfPolygon2D (int[] verts, int nverts) {
			int area = 0;
			for (int i = 0, j = nverts-1; i < nverts; j=i++) {
				int vi = i*4;
				int vj = j*4;
				area += verts[vi+0] * (verts[vj+2]/voxelArea.width) - verts[vj+0] * (verts[vi+2]/voxelArea.width);
			}
			
			return (area+1) / 2;
		}
		
		public static bool Ileft(int a, int b, int c, int[] va, int[] vb, int[] vc)
		{
			return (vb[b+0] - va[a+0]) * (vc[c+2] - va[a+2]) - (vc[c+0] - va[a+0]) * (vb[b+2] - va[a+2]) <= 0;
		}
	}
	
}

